#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp" //消息接口
#include "tf2/LinearMath/Quaternion.hpp" //提供tf2.Quaternion类
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" //消息类型转换
#include "tf2_ros/transform_broadcaster.hpp" //静态坐标广播器
#include <chrono>
using namespace std::chrono_literals;

class DynamicTFBroadcaster : public rclcpp::Node
{
    private:
        std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
        rclcpp::TimerBase::SharedPtr timer_;
    public:
        DynamicTFBroadcaster(): Node("dynamic_tf_broadcaster")
        {
            this->broadcaster_=std::make_shared<tf2_ros::TransformBroadcaster>(this);
            
            timer_=this->create_wall_timer(100ms,std::bind(&DynamicTFBroadcaster::pub_tf,this));
        }

        void pub_tf()
        {
            geometry_msgs::msg::TransformStamped transform_;

            transform_.header.stamp=this->get_clock()->now();

            transform_.header.frame_id="map";
            transform_.child_frame_id="base_link";
            transform_.transform.translation.x=2.0;
            transform_.transform.translation.y=3.0;
            transform_.transform.translation.z=0.0;

            
            tf2::Quaternion q;
            // 角度转弧度
            q.setRPY(0,0,60*M_PI/180);
            transform_.transform.rotation =tf2::toMsg(q);
            this->broadcaster_->sendTransform(transform_);

        };


};

int main (int arg,char* argv[])
{
    rclcpp::init(arg,argv);
    auto nodo=std::make_shared<DynamicTFBroadcaster>();
    rclcpp::spin(nodo);
    rclcpp::shutdown();
    return 0;
}